#include <Tlc5940.h>
#include <tlc_config.h>
#include <Average.h>

//From the bildr article: http://bildr.org/2012/03/servos-tlc5940-arduino
//Requires the Tlc5940 library. http://code.google.com/p/tlc5940arduino/downloads/list
int s0 = 5;
int s1 = 6;
int s2 = 7;
int s3 = 8;

int RED = 0;
int GREEN = 1;
int BLUE = 2;
int MAXIMUM = 4095;

int window = 0;
int value = 0;
int color = RED;
int counter = 0;
int averageSize = 20;
Average* frontAverages[5] = {
    newAverageWithSampleSize(averageSize),
    newAverageWithSampleSize(averageSize),
    newAverageWithSampleSize(averageSize),
    newAverageWithSampleSize(averageSize),
    newAverageWithSampleSize(averageSize)
};
Average* backAverages[5] = {
    newAverageWithSampleSize(averageSize),
    newAverageWithSampleSize(averageSize),
    newAverageWithSampleSize(averageSize),
    newAverageWithSampleSize(averageSize),
    newAverageWithSampleSize(averageSize)
};

void setup() {  
  initializeSonars();
  initSensors();
  Tlc.init(4095);
  Serial.begin(9600);
  for (window = 0; window < 5; window++) {
    setWindowColor(window, 1024, 0, 0);
  }
}

void initSensors() {  
  pinMode(s0, OUTPUT);
  pinMode(s1, OUTPUT);
  pinMode(s2, OUTPUT);
  pinMode(s3, OUTPUT); 

  digitalWrite(s0, LOW);
  digitalWrite(s1, LOW);
  digitalWrite(s2, LOW);
  digitalWrite(s3, LOW);
}

void loop() {
  
  int red = 1024;
  int green = 0;
  int blue = 0;

  for (window = 0; window < 5; window++) {
    int sensor = window * 2;
    int front = constrain(readMux(sensor), 22, 900);
    int back = constrain(readMux(sensor), 22, 900);
    front = frontAverages[window]->averageWithValue(front);
    back = backAverages[window]->averageWithValue(back);
    
    green = map(front, 22, 150, 3095, 0);
    blue = map(back, 22, 150, 3095, 0);

    setWindowColor(window, red, green, blue);
  }

  update();
  counter++;
  if (counter == 3) {
    counter = 0;
  }
}

void readSensor() {
  for(int i = 0; i < 10; i ++){
    Serial.print(i);
    Serial.print(": ");
    Serial.print(readMux(i));
    Serial.print("      ");
  } 
  Serial.println("");
}

void setWindowColor(int window, int red, int green, int blue) {
  int redPin = getRedPin(window);
  int greenPin = getGreenPin(window);
  int bluePin = getBluePin(window);
  
  setColor(redPin, red);
  setColor(greenPin, green);
  setColor(bluePin, blue);
}

int getRedPin(int window) {
  int firstPin = window * 3;
  return firstPin;
}

int getGreenPin(int window) {
  int firstPin = window * 3;
  return firstPin + 1;
}

int getBluePin(int window) {
  int firstPin = window * 3;
  return firstPin + 2;
}

void update() {
  Tlc.update();
  delayMicroseconds(20);
}
void setAllWindowColor(int red, int green, int blue) {
  int window = 0;
  for (window = 0; window < 5; window++) {
    setWindowColor(window, red, green, blue);
  }
}

void setAllColor(int color) {
  int i;
  for (i = 0; i < 16; i++) {
    setColor(i, color);
  }
}

void setColor(int pin, int color) {
  Tlc.set(pin, MAXIMUM - color);
}

int readMux(int channel){
  int controlPin[] = {s0, s1, s2, s3};

  int muxChannel[16][4]={
    {0,0,0,0}, //channel 0
    {1,0,0,0}, //channel 1
    {0,1,0,0}, //channel 2
    {1,1,0,0}, //channel 3
    {0,0,1,0}, //channel 4
    {1,0,1,0}, //channel 5
    {0,1,1,0}, //channel 6
    {1,1,1,0}, //channel 7
    {0,0,0,1}, //channel 8
    {1,0,0,1}, //channel 9
    {0,1,0,1}, //channel 10
    {1,1,0,1}, //channel 11
    {0,0,1,1}, //channel 12
    {1,0,1,1}, //channel 13
    {0,1,1,1}, //channel 14
    {1,1,1,1}  //channel 15
  };

  //loop through the 4 sig
  for(int i = 0; i < 4; i ++){
    digitalWrite(controlPin[i], muxChannel[channel][i]);
  }

  //read the value at the SIG pin
  int val = analogRead(0);

  //return the value
  return val;
}

void initializeSonars() {
  // 0v logic zero initially
  pinMode(2, OUTPUT);  
  digitalWrite(2, LOW);
  delay(320);  
  pullSonarPin();
}

void pullSonarPin() {
  // pull pin "high"
  digitalWrite(2, HIGH);
  delayMicroseconds(25);
  pinMode(2, INPUT);  
  digitalWrite(2, HIGH);
}

